#include "Elephant.h"
#include "Chassis.h"
#include "Remote.h"
#include "math.h"
#include "DJ_Driver.h"
#include "odrive.h"
#include "SBUS.h"

uint8_t MM_flag = 0;
uint8_t shoot_flag = 0;

/**
 * UART1 SBUS
 * UART6 ACTION
 * UART7
 * UART8 PC
 * CAN1 DJ
 * CAN1 RMDS
 * TIM7 MM
 */

/* 送环定时 */
void MM_TIM2_Callback(void)
{
    MM_flag++;
    if (MM_flag == 1)
    {
        HAL_GPIO_WritePin(POWER2_GPIO_Port, POWER2_Pin, GPIO_PIN_SET);
    }
    else if (MM_flag == 2)
    {
        HAL_GPIO_WritePin(POWER2_GPIO_Port, POWER2_Pin, GPIO_PIN_RESET);
    }
    else
    {
        MM_flag = 0;
        HAL_TIM_Base_Stop_IT(&htim2);
    }
}

/* 夹环定时 */
void MM_TIM3_Callback(void)
{
    static uint16_t count;
    static uint8_t jiahuan_flag;

    switch (jiahuan_flag)
    {
    case 0: // 夹环
        if (count++ > 1000)
        {
            count = 0;
            HAL_GPIO_WritePin(POWER3_GPIO_Port, POWER3_Pin, GPIO_PIN_SET);
            jiahuan_flag = 1;
        }
        break;

    case 1: // 上升
        if (count++ > 1000)
        {
            count = 0;
            DJ_SetAngleInc(&DJ_Motor2006[0], -130);
            DJ_SetAngleInc(&DJ_Motor2006[1], -130);
            jiahuan_flag = 0;
            global.step = 2; // 跳转到到I1柱
            HAL_TIM_Base_Stop_IT(&htim3);
        }
        break;

    default:
        break;
    }
}
/* 毫秒定时器 */
void MM_TIM_Callback(void)
{

    static uint16_t count;
    if (count++ > 100)
    {
        count = 0;
        UART_Print("ordive:%.3f ", chassis.odrive_speed);

        // UART_Print("%f %f %f\n", Action.pos_x, Action.pos_y, Action.yaw);
        UART_Print("DJ:%d\n", chassis.DJ_SetAngle[1]);
    }
}

/* 大象初始化 */
inline void Elephant_Init(void)
{
    // HAL_GPIO_WritePin(POWER1_GPIO_Port, POWER1_Pin, GPIO_PIN_SET);
    // HAL_GPIO_WritePin(POWER2_GPIO_Port, POWER2_Pin, GPIO_PIN_SET);
    // HAL_GPIO_WritePin(POWER3_GPIO_Port, POWER3_Pin, GPIO_PIN_SET);
    // HAL_GPIO_WritePin(POWER4_GPIO_Port, POWER4_Pin, GPIO_PIN_SET);
    AttachInterrupt_TIM(&htim7, MM_TIM_Callback);
    AttachInterrupt_TIM(&htim2, MM_TIM2_Callback);
    AttachInterrupt_TIM(&htim3, MM_TIM3_Callback);
    CAN_FilterInit(); // CAN过滤配置
    Chassis_Init();   // 底盘初始化
    Remote_Init();    // 遥控初始化

    /************* 初始化完成 ***********/
    UART_Print("Initialization complete\n");
    HAL_TIM_Base_Start_IT(&htim7);
    HAL_Delay(3000);
}

/* 大象运行循环 */
inline void Elephant_Run(void)
{
    Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
    Elephant_Point();
    Chassis_Run();
    // DJ_SetAngle(&DJ_Motor3508[0], chassis.DJ_SetAngle[0], 1000);
    DJ_SetAngle(&DJ_Motor3508[1], -chassis.DJ_SetAngle[1], 1000);
    odrive_set_speed(chassis.odrive_speed, -chassis.odrive_speed);
    DJ_MotorRun();
}

/*自动射环*/
void Elephant_shoot(void)
{
    DJ_SetAngleInc(&DJ_Motor2006[0], -130);
    DJ_SetAngleInc(&DJ_Motor2006[1], -130);
    shoot_flag = 1;
    HAL_TIM_Base_Start_IT(&htim2);
}

/*大象全自动*/
void Elephant_Point(void)
{
    switch (global.step)
    {
    case 0: // 取环第一个点
        Chassis_SetPos(-4692, 508, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));

        if (Dis <= 100)
        {
            /* 下降 */
            DJ_SetAngleInc(&DJ_Motor2006[0], 130);
            DJ_SetAngleInc(&DJ_Motor2006[1], 130);
            global.step = 1;
        }
        break;

    case 1: // 取环第二个点
        Chassis_SetPos(-5176, -100, -0.684268);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.odrive_speed = 20.231;

        if (Dis <= 100)
        {
            HAL_TIM_Base_Start_IT(&htim3);
            global.step = 99; // 跳到空状态
        }
        break;

    case 2: // 一型柱1
        Chassis_SetPos(-2982, 785, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.DJ_SetAngle[1] = 20;
        if (Dis <= 10)
        {
            if (shoot_flag == 0)
            {
                shoot_flag = 1;
                HAL_TIM_Base_Start_IT(&htim2);
            }
            if (MM_flag == 2)
            {
                global.step = 3;
                shoot_flag = 0;
            }
        }
        break;

    case 3: // 一型柱2
        // Chassis_SetPos(0, 785, 0);
        Chassis_SetPos(235, 785, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.odrive_speed = 20.231;
        chassis.DJ_SetAngle[1] = 20;
        if (Dis <= 100)
        {

            if (shoot_flag == 0)
                Elephant_shoot();
            if (MM_flag == 2)
            {
                global.step = 4;
                shoot_flag = 0;
            }
        }
        break;

    case 4: // 一型柱3
        Chassis_SetPos(3400, 785, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.odrive_speed = 20.231;
        chassis.DJ_SetAngle[1] = 20;
        if (Dis <= 10)
        {

            if (shoot_flag == 0)
                Elephant_shoot();
            if (MM_flag == 2)
            {
                global.step = 5;
                shoot_flag = 0;
            }
        }
        break;

    case 5: // 二型柱2
        Chassis_SetPos(1502, 785, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.odrive_speed = 32.308;
        chassis.DJ_SetAngle[1] = 20;
        if (Dis <= 10)
        {

            if (shoot_flag == 0)
                Elephant_shoot();
            if (MM_flag == 2)
            {
                global.step = 6;
                shoot_flag = 0;
            }
        }
        break;

    case 6: // 二型柱1
        Chassis_SetPos(-1128, 785, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.odrive_speed = 32.308;
        chassis.DJ_SetAngle[1] = 20;
        if (Dis <= 10)
        {
            if (shoot_flag == 0)
                Elephant_shoot();
            if (MM_flag == 2)
            {
                global.step = 7;
                shoot_flag = 0;
            }
        }
        break;

    case 7: // 三型柱1
        Chassis_SetPos(235, 785, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.odrive_speed = 42.923;
        chassis.DJ_SetAngle[1] = 28;
        if (Dis <= 10)
        {

            if (shoot_flag == 0)
                Elephant_shoot();
            if (MM_flag == 2)
            {
                global.step = 8;
                shoot_flag = 0;
            }
        }
        break;

    case 8: // 回到起点
        Chassis_SetPos(0, 0, 0);
        Dis = sqrt((chassis.setPx - Action.pos_x) * (chassis.setPx - Action.pos_x) + (chassis.setPy - Action.pos_y) * (chassis.setPy - Action.pos_y));
        chassis.odrive_speed = 0;
        chassis.DJ_SetAngle[1] = 0;
        if (Dis <= 10)
        {
            global.step = 9;
        }
        break;

    case 99:
        /* 空等待 */
        break;

    default:
        break;
    }
}
